#include "VI5300_UserPlatform.h"
#include "VI5300_API.h"
#include "zino.h"
#include "fc_error.h"
#include "sensordata.h"
#include <rtthread.h>
#include <rtdevice.h>
#define DBG_TAG "VI5300"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

static struct rt_i2c_bus_device *vi_i2c_bus = RT_NULL; /* I2C总线设备句柄 */

#define VI5300_ECO_2V1 1

#ifdef VI5300_ECO_2V1

VI5300_Status I2C_2V1_WriteOneReg(uint8_t reg, uint8_t value)
{
	struct rt_i2c_msg vi_msgs;
	uint8_t buf[2] = {reg, value};
	vi_msgs.addr = VI5300_DEVICE_ADDR >> 1;
	vi_msgs.buf = buf;
	vi_msgs.len = 2;
	vi_msgs.flags = RT_I2C_WR;

	// struct rt_i2c_bit_ops *ops = (struct rt_i2c_bit_ops *)vi_i2c_bus->priv;
	// ops->delay_us = 0;
	if (rt_i2c_transfer(vi_i2c_bus, &vi_msgs, 1) != 1)
	{
		return VI5300_ERROR;
	}
	return RT_EOK;
}

VI5300_Status I2C_2V1_ReadOneReg(uint8_t reg, uint8_t *value)
{
	struct rt_i2c_msg vi_msgs[2];

	vi_i2c_bus = (struct rt_i2c_bus_device *)rt_device_find("i2c1");
	vi_msgs[0].addr = VI5300_DEVICE_ADDR >> 1;
	vi_msgs[0].buf = &reg;
	vi_msgs[0].len = 1;
	vi_msgs[0].flags = RT_I2C_WR;

	vi_msgs[1].addr = VI5300_DEVICE_ADDR >> 1;
	vi_msgs[1].buf = value;
	vi_msgs[1].len = 1;
	vi_msgs[1].flags = RT_I2C_RD;
	// struct rt_i2c_bit_ops *ops = (struct rt_i2c_bit_ops *)vi_i2c_bus->priv;

	if (rt_i2c_transfer(vi_i2c_bus, vi_msgs, 2) != 2)
	{
		return VI5300_ERROR;
	}
	return RT_EOK;
}

VI5300_Status I2C_WriteXByteWraper(uint8_t reg, uint8_t *pValue, uint8_t tlen)
{
	struct rt_i2c_msg vi_msgs[2];

	vi_msgs[0].addr = VI5300_DEVICE_ADDR >> 1;
	vi_msgs[0].buf = &reg;
	vi_msgs[0].len = 1;
	vi_msgs[0].flags = RT_I2C_WR;

	vi_msgs[1].addr = VI5300_DEVICE_ADDR >> 1;
	vi_msgs[1].buf = pValue;
	vi_msgs[1].len = tlen;
	vi_msgs[1].flags = RT_I2C_WR | RT_I2C_NO_START;
	// struct rt_i2c_bit_ops *ops = (struct rt_i2c_bit_ops *)vi_i2c_bus->priv;
	// ops->delay_us = 0;
	if (rt_i2c_transfer(vi_i2c_bus, vi_msgs, 2) != 2)
	{
		return VI5300_ERROR;
	}
	return RT_EOK;
}

VI5300_Status I2C_ReadXByteWraper(uint8_t reg, uint8_t *value, uint8_t tlen)
{
	struct rt_i2c_msg vi_msgs[2];
	vi_msgs[0].addr = VI5300_DEVICE_ADDR >> 1;
	vi_msgs[0].buf = &reg;
	vi_msgs[0].len = 1;
	vi_msgs[0].flags = RT_I2C_WR;

	vi_msgs[1].addr = 0xD8 >> 1;
	vi_msgs[1].buf = value;
	vi_msgs[1].len = tlen;
	vi_msgs[1].flags = RT_I2C_RD;
	// struct rt_i2c_bit_ops *ops = (struct rt_i2c_bit_ops *)vi_i2c_bus->priv;
	// ops->delay_us = 0;
	if (rt_i2c_transfer(vi_i2c_bus, vi_msgs, 2) != 2)
	{
		return VI5300_ERROR;
	}
	return RT_EOK;
}

#endif

VI5300_Status WriteOneReg(uint8_t addr, uint8_t value)
{
	return I2C_2V1_WriteOneReg(addr, value);
}

VI5300_Status ReadOneReg(uint8_t addr, uint8_t *value)
{
	return I2C_2V1_ReadOneReg(addr, value);
}

VI5300_Status WriteCommand(uint8_t cmd)
{
	return WriteOneReg(VI5300_REG_CMD, cmd);
}

VI5300_Status I2C_WriteXBytes(uint8_t startaddr, uint8_t *buf, uint8_t len)
{
	return I2C_WriteXByteWraper(startaddr, buf, len);
}

VI5300_Status I2C_ReadXBytes(uint8_t startaddr, uint8_t *buf, uint8_t len)
{
	return I2C_ReadXByteWraper(startaddr, buf, len);
}

VI5300_CALIBRATION_DATA cali_data;
VI5300_Dist_TypeDef result1, result2;
VI5300_Status ret;
uint16_t iCount = 0;
// static bool VI5300_INIT = false;

static uint8_t Calibration_data[10] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

void StoreCalibrationDataToFile(VI5300_CALIBRATION_DATA cali_data)
{
	// uint8_t index=0;
	union byte_int16
	{
		uint8_t byte[2];
		int16_t int16_data;
	} byte_to_int16;

	Calibration_data[0] = cali_data.Cali_Flag;
	Calibration_data[1] = cali_data.xtalk_cal;
	byte_to_int16.int16_data = cali_data.offset_cal;
	Calibration_data[2] = byte_to_int16.byte[0];
	Calibration_data[3] = byte_to_int16.byte[1];
	byte_to_int16.int16_data = cali_data.RefTof;
	Calibration_data[4] = byte_to_int16.byte[0];
	Calibration_data[5] = byte_to_int16.byte[1];
}

void ReadCalibrationDataFromFile(VI5300_CALIBRATION_DATA *cali_data)
{
	union byte_int16
	{
		uint8_t byte[2];
		int16_t int16_data;
	} byte_to_int16;

	cali_data->Cali_Flag = Calibration_data[0];
	cali_data->xtalk_cal = Calibration_data[1];
	byte_to_int16.byte[0] = Calibration_data[2];
	byte_to_int16.byte[1] = Calibration_data[3];
	cali_data->offset_cal = byte_to_int16.int16_data;
	byte_to_int16.byte[0] = Calibration_data[4];
	byte_to_int16.byte[1] = Calibration_data[5];
	cali_data->RefTof = byte_to_int16.int16_data;
}

void VI5300_data_update()
{

	static uint8_t try = 0;
	while (try++ < 5)
	{
		uint8_t vi5300_id = VI5300_Device_Check();
		if (vi5300_id == 0XD8)
		{
			// if (!VI5300_INIT)
			// {
			VI5300_init();

			if ((cali_data.Cali_Flag & 0xF0) != 0x10)
			{
				// 标定在第一次开机执行，或者工厂模式执行，后面不能执行
				VI5300_Start_RefTof_Calibration(&offset_data);
				cali_data.Cali_Flag |= 0x10; // RefTof&offset calibration flag
				cali_data.RefTof = offset_data.RefTof;
				cali_data.offset_cal = offset_data.offset_cal;
			}
			else
			{
				offset_data.RefTof = cali_data.RefTof;
				offset_data.offset_cal = cali_data.offset_cal;
			}

			VI5300_Config_RefTof(offset_data.RefTof);
			VI5300_Start_Continuous_Measure();

			// VI5300_INIT = true;
			// rt_kprintf("cali_data.Cali_Flag = %d\t", cali_data.Cali_Flag);
			// rt_kprintf("cali_data.RefTof = %d\t", cali_data.RefTof);
			// rt_kprintf("cali_data.offset_cal = %d\n", cali_data.offset_cal);
			LOG_I("VI5300 INIT OK!");
			FC_ERROR_RESET(FC_LASER_HARDFAULT);
			break;
			// }
		}
		else
		{
			LOG_E("VI5300 CHECK ID ERROR! id:%02x, %d", vi5300_id, try);
			LOG_E("VI5300 INIT ERROR!");
			// VI5300_INIT = false;
			cali_data.Cali_Flag = 0;
			FC_ERROR_SET(FC_LASER_HARDFAULT);
		}
	}

	while (1)
	{
		ret = VI5300_Get_Measure_Data(&result1);
		if (!ret)
		{
			sensorData.tof.range = (int16_t)result1.millimeter;
			if(sensorData.tof.range < 0)
				sensorData.tof.range = 0;
			sensorData.tof.quality = result1.confidence;
		}
		rt_thread_mdelay(10);
	}
}

int vi53_task_entry(void)
{
	rt_thread_t vi53_data_thread = rt_thread_create("vi53_data_thread", VI5300_data_update, RT_NULL, 1024, 10, 10);
	if (vi53_data_thread != RT_NULL)
	{
		rt_thread_startup(vi53_data_thread);
	}
	else
	{
		LOG_E("sensor_data_thread create failed.");
	}
	LOG_I("vi53_task_entry.");
	return 0;
}

// ZINO_APP_EXPORT(vi53_task_entry);